#include "usb_task.h"
#include "cmsis_os.h"
#include "usb_device.h"
#include "detect_task.h"
#include "INS_task.h"
#include "usbd_cdc_if.h"
#include "math.h"
#include "arm_math.h"
#include "user_lib.h"
#include "gimbal_task.h"
#include "referee.h"
#include "CRC8_CRC16.h"

//0x5A为自瞄,0x5B为导航
usb_control_t usb_control;

void USB_STM32_Tx_To_PC(fp32 roll, fp32 pitch, fp32 yaw, usb_control_t *usb_tx_pc);

void usb_task(void const *argument)
{
	MX_USB_DEVICE_Init();

	while (1)
	{
		const fp32 *gyro_point = get_INS_angle_point();
		USB_STM32_Tx_To_PC(gyro_point[1], gimbal_control.gimbal_pitch_motor.relative_angle, gyro_point[0], &usb_control);
//		USB_STM32_Tx_To_PC(gyro_point[1], gimbal_control.gimbal_pitch_motor.relative_angle, gimbal_control.gimbal_yaw_motor.relative_angle);
		osDelay(1);
	}
}

//待改，裁判系统
//导航帧头是A5
void Transmit_RD(uint8_t *Nano_Buf, usb_control_t *usb_ld_ctrl)
{
	if (Nano_Buf[0] == 0xB5)
	{
		usb_ld_ctrl->ld_ctrl.Head = Nano_Buf[0];
	    usb_ld_ctrl->ld_ctrl.mode = Nano_Buf[1];
	    usb_ld_ctrl->ld_ctrl.CRC_16 = Nano_Buf[26];

		uint32_t Linear_Twist_X_hex = (Nano_Buf[5] << 24) | (Nano_Buf[4] << 16) | (Nano_Buf[3] << 8) | Nano_Buf[2];
	    uint32_t Linear_Twist_Y_hex = (Nano_Buf[9] << 24) | (Nano_Buf[8] << 16) | (Nano_Buf[7] << 8) | Nano_Buf[6];
	    uint32_t Linear_Twist_Z_hex = (Nano_Buf[13] << 24) | (Nano_Buf[12] << 16) | (Nano_Buf[11] << 8) | Nano_Buf[10];
	    uint32_t Angular_Twist_X_hex = (Nano_Buf[17] << 24) | (Nano_Buf[16] << 16) | (Nano_Buf[15] << 8) | Nano_Buf[14];
	    uint32_t Angular_Twist_Y_hex = (Nano_Buf[21] << 24) | (Nano_Buf[20] << 16) | (Nano_Buf[19] << 8) | Nano_Buf[18];
	    uint32_t Angular_Twist_Z_hex = (Nano_Buf[25] << 24) | (Nano_Buf[24] << 16) | (Nano_Buf[23] << 8) | Nano_Buf[22];

		usb_ld_ctrl->ld_ctrl.Linear_Twist_X = *((fp32*)&Linear_Twist_X_hex);
	    usb_ld_ctrl->ld_ctrl.Linear_Twist_Y = *((fp32*)&Linear_Twist_Y_hex);
	    usb_ld_ctrl->ld_ctrl.Linear_Twist_Z = *((fp32*)&Linear_Twist_Z_hex);
	    usb_ld_ctrl->ld_ctrl.Angular_Twist_X = *((fp32*)&Angular_Twist_X_hex);
	    usb_ld_ctrl->ld_ctrl.Angular_Twist_Y = *((fp32*)&Angular_Twist_Y_hex);
	    usb_ld_ctrl->ld_ctrl.Angular_Twist_Z = *((fp32*)&Angular_Twist_Z_hex);
	}
}

void USB_STM32_Tx_To_PC(fp32 roll, fp32 pitch, fp32 yaw, usb_control_t *usb_tx_pc)
{
	bool_t *framepoint;
	static uint8_t usart_tx_buf[TX_BUF_LENGTH]; // Add 4 bytes for frame header, frame tail, and CRC
	memset(usart_tx_buf, 0, TX_BUF_LENGTH);
	int32_t cnt = 0;
	usb_tx_pc->stm32_to_pc.framehead = 0x5A;
	if (robot_state_t.robot_id < 100)
	{
		usb_tx_pc->stm32_to_pc.detect_color = 1;
		usb_tx_pc->stm32_to_pc.ones_own_outpost_HP = game_robot_HP_t.red_outpost_HP;
		usb_tx_pc->stm32_to_pc.others_outpost_HP = game_robot_HP_t.blue_outpost_HP;
		if (map_data_t.target_position_x > 20.0f)
		{
			usb_tx_pc->stm32_to_pc.run_mode = 1;
		}
		else
		{
			usb_tx_pc->stm32_to_pc.run_mode = 0;
		}
	}
	else
	{
		usb_tx_pc->stm32_to_pc.detect_color = 0;
		usb_tx_pc->stm32_to_pc.ones_own_outpost_HP = game_robot_HP_t.blue_outpost_HP;
		usb_tx_pc->stm32_to_pc.others_outpost_HP = game_robot_HP_t.red_outpost_HP;
		if (map_data_t.target_position_x < 10.0f)
		{
			usb_tx_pc->stm32_to_pc.run_mode = 1;
		}
		else
		{
			usb_tx_pc->stm32_to_pc.run_mode = 0;
		}
	}
	usb_tx_pc->stm32_to_pc.task_mode = 0;//����
	usb_tx_pc->stm32_to_pc.reset_tracker = 0;
	usb_tx_pc->stm32_to_pc.is_play = 0;
	usb_tx_pc->stm32_to_pc.change_target = 0;
	usb_tx_pc->stm32_to_pc.reserved = 0;
	usb_tx_pc->stm32_to_pc.roll = roll;
	usb_tx_pc->stm32_to_pc.pitch = pitch;
	usb_tx_pc->stm32_to_pc.yaw = yaw;
	usb_tx_pc->stm32_to_pc.game_progress = game_state_t.game_progress;
	usb_tx_pc->stm32_to_pc.stage_remain_time = game_state_t.stage_remain_time;
	usb_tx_pc->stm32_to_pc.remain_HP = robot_state_t.current_HP;
	usb_tx_pc->stm32_to_pc.dif_yaw = usb_tx_pc->cv_ctrl.dif_yaw;
	usb_tx_pc->stm32_to_pc.game_time = 1;
	usb_tx_pc->stm32_to_pc.timestamp = xTaskGetTickCount();//获取单片机时间戳
	framepoint = (bool_t *)&usb_tx_pc->stm32_to_pc;
	for (int32_t i = 0; i < 38; i++)
	{
		usart_tx_buf[cnt] = *(framepoint + i);
		usb_tx_pc->tx_buf[cnt] = usart_tx_buf[cnt];
		cnt++;
	}

	usart_tx_buf[0] = usb_tx_pc->stm32_to_pc.framehead;
	append_CRC16_check_sum(usart_tx_buf, TX_BUF_LENGTH);
	CDC_Transmit_FS(usart_tx_buf, sizeof(usart_tx_buf));

}

fp32 hex_to_float(uint8_t a, uint8_t b, uint8_t c, uint8_t d)
{
	uint32_t number = ((uint32_t)a << 24) | ((uint32_t)b << 16) | ((uint32_t)c << 8) | (uint32_t)d;
	int32_t sign = (number & 0x80000000) ? -1 : 1;
	int32_t exponent = ((number >> 23) & 0xff) - 127;
	fp32 mantissa = 1.0f + ((fp32)(number & 0x7fffff) / (fp32)0x7fffff);
	if (exponent == -127)
	{
		return sign * ldexp(mantissa, exponent);
	}
	else if (exponent == 128)
	{
		if ((number & 0x7fffff) == 0)
		{
			return sign * INFINITY;
		}
		else
		{
			return NAN;
		}
	}

	// Calculate the final floating-point value
	return sign * ldexp(mantissa, exponent);
}

//小电脑发送给STM32
void PC_Tranmit_32(uint8_t *Nano_Buf, usb_control_t *usb_cv_ctrl)
{
    if(Nano_Buf[0] == 0xA5)
    {

        usb_cv_ctrl->cv_ctrl.Head = Nano_Buf[0];
        usb_cv_ctrl->cv_ctrl.state = Nano_Buf[1];
        usb_cv_ctrl->cv_ctrl.id = Nano_Buf[2];
        usb_cv_ctrl->cv_ctrl.armors_num = Nano_Buf[3];
        usb_cv_ctrl->cv_ctrl.is_fire = Nano_Buf[4];
		usb_cv_ctrl->cv_ctrl.back = Nano_Buf[5];

		usb_cv_ctrl->cv_ctrl.x = hex_to_float(Nano_Buf[9],Nano_Buf[8],Nano_Buf[7],Nano_Buf[6]);
		usb_cv_ctrl->cv_ctrl.y = fp32_constrain(hex_to_float(Nano_Buf[13],Nano_Buf[12],Nano_Buf[11],Nano_Buf[10]), gimbal_control.gimbal_pitch_motor.min_relative_angle, gimbal_control.gimbal_pitch_motor.max_relative_angle);
        usb_cv_ctrl->cv_ctrl.z = hex_to_float(Nano_Buf[17],Nano_Buf[16],Nano_Buf[15],Nano_Buf[14]);
        usb_cv_ctrl->cv_ctrl.yaw = hex_to_float(Nano_Buf[21],Nano_Buf[20],Nano_Buf[19],Nano_Buf[18]);
        usb_cv_ctrl->cv_ctrl.vx = hex_to_float(Nano_Buf[25],Nano_Buf[24],Nano_Buf[23],Nano_Buf[22]);
        usb_cv_ctrl->cv_ctrl.vy = hex_to_float(Nano_Buf[29],Nano_Buf[28],Nano_Buf[27],Nano_Buf[26]);
        usb_cv_ctrl->cv_ctrl.vz = hex_to_float(Nano_Buf[33],Nano_Buf[32],Nano_Buf[31],Nano_Buf[30]);
        usb_cv_ctrl->cv_ctrl.v_yaw = hex_to_float(Nano_Buf[37],Nano_Buf[36],Nano_Buf[35],Nano_Buf[34]);
        usb_cv_ctrl->cv_ctrl.r1 = hex_to_float(Nano_Buf[41],Nano_Buf[40],Nano_Buf[39],Nano_Buf[38]);
        usb_cv_ctrl->cv_ctrl.r2 = hex_to_float(Nano_Buf[45],Nano_Buf[44],Nano_Buf[43],Nano_Buf[42]);
        usb_cv_ctrl->cv_ctrl.dz = hex_to_float(Nano_Buf[49],Nano_Buf[48],Nano_Buf[47],Nano_Buf[46]);

        usb_cv_ctrl->cv_ctrl.cap_timestamp = ((Nano_Buf[53] << 24 | Nano_Buf[52] << 16 | Nano_Buf[51] << 8 | Nano_Buf[50]));
        usb_cv_ctrl->cv_ctrl.t_offset = ((Nano_Buf[55] << 8 | Nano_Buf[54]));
        usb_cv_ctrl->cv_ctrl.checksum = ((Nano_Buf[57] << 8 | Nano_Buf[56]));
    }
}

cv_ctrl_t *get_cv_point(void)
{
	return &usb_control.cv_ctrl;
}

const ld_ctrl_t *get_ld_point(void)
{
	return  &usb_control.ld_ctrl;
}
